#include "narrow_phase.h"

#include <device_launch_parameters.h>
#include <device_functions.h>

namespace gc {
	template<>
	__global__ void narrowPhase(int broadSize, CollisionPair *broadCPs, SimpleSphere *prims, MPhys::RigidBody *rigids) {
		int idx = blockIdx.x*blockDim.x + threadIdx.x;
		if (idx >= broadSize) return;

		int i = broadCPs[idx].primIdx1, j = broadCPs[idx].primIdx2;

		const Point3f n_ij = prims[j].center - prims[i].center;
		Float dist = length(n_ij);
		const Point3f norm = n_ij * (1. / dist);
		Float totalRadius = prims[i].radius + prims[j].radius;
		
		if (dist < (totalRadius - EPS)) {
			// rectify prim overlap
			prims[j].center = prims[i].center + norm * totalRadius;
			rigids[j].position = prims[j].center;
			collide(norm, rigids[i], rigids[j]);
		}
		else if (dist <= (totalRadius + EPS)) {
			collide(norm, rigids[i], rigids[j]);
		}
		
	}
}